/**
 * @author Remi Destigny
 * 
 * Utily class providing math utilities 
 */
public class MathUtils {
	
	/**
	 * Transform a given point to the robot system of coordinates
	 * @param r The robot 
	 * @param point The point to transform
	 * @return The transformed point 
	 */
	static public Vector pathPointToRobotCoordinates(Robot r, Vector point) {
		Quaternion q = r.getOrientation();
		Double teta = q.getPsi();
		Vector position = r.getPosition();
		double newX = ( point.x - position.x ) * Math.cos(teta) + 
				( point.y - position.y ) * Math.sin(teta);
		double newY = -1 * ( point.x - position.x ) * Math.sin(teta) + 
				( point.y - position.y ) * Math.cos(teta);
		return new Vector(newX, newY);
	}
	
	/**
	 * Compute if two values are equals with an epsilon value
	 * @param v1 First value to compare
	 * @param v2 Second value to compute
	 * @param epsilon The epsilon value 
	 * @return
	 */
	static public boolean epsilonEqual(double v1, double v2, double epsilon) {
		return v1 >= v2 - epsilon && v1 <= v2 + epsilon;
	}
	
}
